/*
 * GPS.cpp
 *
 *  Created on: Feb 9, 2014
 *      Author: ppks
 */

#include "common.h"
#include "GPS.h"
#if (GPS_PROTOCOL == UBLOX)
#include "GPS_UBLOX.h"
#endif /* #if (GPS_PROTOCOL == UBLOX) */
#include "GPS_PID.h"
#include "GPS_Configuration.h"
#include "FlightController_Common.h"
#include "EEPROM.h"

/******************************************************************************/
/* Global Variables Declaration                                               */
/******************************************************************************/
WaypointRecord_t WaypointRecord;

uint8_t  GPS_Enabled;                   /* Flag that tells whether the GPS is enabled or not */
uint8_t  GPS_Present;
uint8_t  GPS_Update;                    /* a binary toogle to distinct a GPS position update */
uint8_t  GPS_NumSat;                    /* A number of detected satellites */

int32_t  GPS_CurrentCoord[2];           /* Current GPS coordinates - longitude and latitude */
int32_t  GPS_HomeCoord[2];              /* Coordinates of home location */
int32_t  GPS_HoldCoord[2];              /* Coordinates for hold position */
int32_t  GPS_NextWPCoord[2];            /* Next WP Coordinates when in NAV mode */
int32_t  GPS_PrevWPCoord[2];
int32_t  GPS_POICoord[2];               /* Coordinates of the POI: used for head adjustment */
uint16_t GPS_DistanceToHome;            /* Distance to home  - unit: meter */
uint32_t GPS_DistanceToWP;              /* distance between plane and next_WP in cm */
int16_t  GPS_DirectionToHome;           /* Direction to home - unit: degree */
int32_t  GPS_DirectionToPOI;            /* Direction to the POI used in head adjustment */
uint16_t GPS_Altitude;                  /* GPS altitude      - unit: meter */
uint16_t GPS_Speed;                     /* GPS speed         - unit: cm/s */
uint16_t GPS_GroundCourse;              /* GPS Ground Course - unit: degree * 10 */
int16_t  GPS_Angle[2];                  /* The angles that must be applied for GPS correction */

uint8_t  NAV_state = NAV_STATE_NONE;     /* State of the navigation engine */
uint8_t  NAV_error = NAV_ERROR_NONE;

int16_t  NAV_Bank[2];                   /* The desired bank towards North (Positive) or South (Negative) : latitude & longitude */
int32_t  GPS_TargetBearing;             /* This is the angle from the copter to the "next_WP" location in degrees * 100 */
int32_t  GPS_OriginalTargetBearing;     /* Crosstrack: deg * 100, The original angle to the next_WP when the next_WP was set */
int16_t  GPS_TakeoffBearing;            /* saves the bearing at takeof (1deg = 1) used to rotate to takeoff direction when arrives at home */
int16_t  GPS_CosstrackError;            /* The amount of angle correction applied to target_bearing to bring the copter back on its optimum path */

uint8_t  NAV_PauseAtStep = 0;           /* This contains the mission step where poshold paused the runing mission. */
uint8_t  NAV_NextStep = 1;              /* The mission step which is upcoming it equals with the WaypointRecord stored in EEPROM */
int16_t  NAV_JumpTimes = -10;

/******************************************************************************/
/* Local Variables Declaration                                                */
/******************************************************************************/
static float GPS_dTnav;         /* Delta Time in milliseconds for navigation computations, updated with every good GPS read */
static float GPS_scaleLonDown;  /* this is used to offset the shrinking longitude as we go towards the poles */

static int32_t  GPS_AltitudeHold;
static int16_t  GPS_ActualSpeed[2] = {0, 0};
static int16_t  GPS_RateError[2];
static int32_t  GPS_PosError[2];

#if defined(GPS_FILTERING)
    static uint8_t GPS_filter_index = 0;
    static int32_t GPS_filter[2][GPS_FILTER_VECTOR_LENGTH];
    static int32_t GPS_filter_sum[2];
    static int32_t GPS_read[2];
    static int32_t GPS_filtered[2];
    static int32_t GPS_degree[2];       /* the lat lon degree without any decimals (lat/10 000 000) */
    static uint16_t fraction3[2];
#endif /* GPS_FILTERING */

static int32_t  GPS_CoordLead[2];   /* Lead filtered GPS coordinates */

static int32_t  NAV_TargetAltitude;
static int32_t  NAV_OriginalAltitude;
static uint32_t Alt_ChangeTimer;
static int8_t   Alt_ChangeFlag;
static uint32_t Alt_ChangeIntegrator;
static uint8_t  NAV_LandDetectCounter;
static uint16_t NAV_WP_SpeedGov;               /* used for slow speed wind up when start navigation */

static uint32_t NAV_TimerStop = 0;      /* common timer used in navigation (contains the desired stop time in millis() */
static uint16_t NAV_HoldTime;           /* time in seconds to hold position */

static uint32_t GPS_LastFrameTimeStamp; /* Timestamp to remember last GPS packet received. Used to determine when we lost GPS signal */
static uint32_t GPS_LandSettleTimer;

/******************************************************************************/
/* Local Functions Declaration                                                */
/******************************************************************************/
static void GPS_AdjustHeading(void);
static void GPS_CalcLongitudeScaling(int32_t lattitude);
static void GPS_CalcDistance(int32_t* fromCoord, int32_t * toCoord, uint32_t* dist);
static void GPS_CalcBearing(int32_t* fromCoord, int32_t * toCoord, int32_t* bearing);
static void GPS_CalcVelocity(void);
static void GPS_CalcLocationError( int32_t* targetCoord, int32_t* currentCoord );
static void GPS_CalcPositionHold(void);
static void GPS_CalcNavRate(uint16_t max_speed);
static void GPS_UpdateCrosstrack(void);
static bool GPS_CheckMissedWP(void);
static void GPS_ClearNewAltitude(void);
static void GPS_ForceNewAltitude(int32_t _new_alt);
static void GPS_CheckIfLanded(void);
static void GPS_ProcessNavigation(void);
static int32_t GPS_LeadFilter_GetPosition(int32_t pos, int16_t vel, int16_t prevVel, float lag_in_seconds);
static uint16_t GPS_CalcDesiredSpeed(uint16_t max_speed, bool slow);
static int32_t wrap_36000(int32_t ang);
static int32_t wrap_18000(int32_t ang);
static int32_t GPS_GetAltitudeError(void);
static void GPS_AbortMission(unsigned char error_code);
static int16_t constrain_int16(int16_t amt, int16_t low, int16_t high);

/******************************************************************************/
/* Function Code                                                              */
/******************************************************************************/
/*******************************************************************************
* Function Name: GPS_Init
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void GPS_Init(void)
{
#if (GPS_PROTOCOL == UBLOX)
    GPS_UBX_Init();
#endif /* (GPS_PROTOCOL == UBLOX) */

    /* Attempt to read new data from GPS for 5 times and update GPS_Present flag */
    for (uint8_t j = 0; j <= 5; j++)
    {
        GPS_GetNewData();
        LEDPIN_ON
        delay(20);
        LEDPIN_OFF
        delay(80);
    }

    GPS_Enabled = GPS_Present;

    /* Init variables */
    GPS_Angle[LAT] = 0;
    GPS_Angle[LON] = 0;
}

/*******************************************************************************
* Function Name: GPS_GetNewData
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void GPS_GetNewData(void)
{
#if (GPS_PROTOCOL == UBLOX)

    if (GPS_UBX_GetNewFrame())
    {
        /* Toggle GPS Update binary flag */
        GPS_Update ^= 1;

        /* Record last received frame timestamp */
        GPS_LastFrameTimeStamp = millis();

        /* Process Navigation */
        GPS_ProcessNavigation();
    }

    /* Check if we lost satellites */
    if ((millis() - GPS_LastFrameTimeStamp) > GPS_LOST_SIGNAL_TIMEOUT_MS)
    {
        /* No update since 1200ms clear fix... */
        globalFlags.GPS_FIX = 0;
        GPS_NumSat = 0;
    }


#endif /* (GPS_PROTOCOL == UBLOX) */
}

/********************************************************************************
* Function Name: NAV_ProcessNavigation
*********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*********************************************************************************/
static void GPS_ProcessNavigation(void)
{
    static uint32_t navLoopTimeStamp;   /* Time for calculating x,y speed and navigation pids */
    uint8_t axis;

    /* No fix, or les than 5 sats --> no GPS functionality */
    if ((globalFlags.GPS_FIX) && (GPS_NumSat >= GPS_STABLE_MIN_SATELLITE_NUM))
    {
    #if !defined(DONT_RESET_HOME_AT_ARM)
        if (!globalFlags.ARMED)
        {
            globalFlags.GPS_FIX_HOME = 0;
        }
    #endif /* !defined(DONT_RESET_HOME_AT_ARM) */

        if ((!globalFlags.GPS_FIX_HOME) && (globalFlags.ARMED))
        {
            GPS_ResetHomePosition();
        }

    #if defined(GPS_FILTERING)
        /* Apply moving average filter to GPS data */
        GPS_filter_index = (GPS_filter_index+1) % GPS_FILTER_VECTOR_LENGTH;
        for (axis = 0; axis< 2; axis++)
        {
            GPS_read[axis] = GPS_CurrentCoord[axis]; //latest unfiltered data is in GPS_latitude and GPS_longitude
            GPS_degree[axis] = GPS_read[axis] / 10000000;  // get the degree to assure the sum fits to the int32_t

            /* How close we are to a degree line ? its the first three digits from the fractions of degree
             * later we use it to Check if we are close to a degree line, if yes, disable averaging, */
            fraction3[axis] = (GPS_read[axis]- GPS_degree[axis]*10000000) / 10000;

            GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index];
            GPS_filter[axis][GPS_filter_index] = GPS_read[axis] - (GPS_degree[axis]*10000000);
            GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index];
            GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis]*10000000);

            if ( NAV_state == NAV_STATE_HOLD_INFINIT || NAV_state == NAV_STATE_HOLD_TIMED)
            {
                /* We use gps averaging only in poshold mode... */
                if ((fraction3[axis] > 1) && (fraction3[axis] < 999))
                {
                    GPS_CurrentCoord[axis] = GPS_filtered[axis];
                }
            }
        }
    #endif /* defined(GPS_FILTERING) */

        GPS_dTnav = (float)(millis() - navLoopTimeStamp) / 1000.0;
        navLoopTimeStamp = millis();

        /* prevent runup from bad GPS */
        GPS_dTnav = min(GPS_dTnav, 1.0);

        /* calculate distance and bearings for GUI and other stuff continuously - From home to copter */
        if (globalFlags.GPS_FIX_HOME)
        {
            int32_t dir;
            uint32_t dist;
            GPS_CalcBearing(GPS_CurrentCoord, GPS_HomeCoord, &dir);
            GPS_CalcDistance(GPS_CurrentCoord, GPS_HomeCoord, &dist);
            GPS_DistanceToHome = dist / 100;
            GPS_DirectionToHome = dir / 100;
        }
        else
        {
            /* If we don't have home set, do not display anything */
            GPS_DistanceToHome = 0;
            GPS_DirectionToHome = 0;
        }

        /* Check fence setting and execute RTH if necessary */
        /* TODO: autolanding */
        if ((GPS_Config.fence > 0) && (GPS_Config.fence < GPS_DistanceToHome) && (globalFlags.GPS_MODE != GPS_MODE_RTH) )
        {
            GPS_InitRTH();
        }

        /* calculate the current velocity based on GPS coordinates continuously to get a valid speed
         * at the moment when we start navigating */
        GPS_CalcVelocity();

        /* TODO : navigation code */
        if (globalFlags.GPS_MODE != GPS_MODE_NONE)
        {
            /* ok we are navigating ###0002 */
            /* do gps nav calculations here, these are common for nav and poshold   */
            GPS_CalcBearing(GPS_CurrentCoord, GPS_NextWPCoord, &GPS_TargetBearing);
            if (GPS_Config.lead_filter)
            {
                GPS_CalcDistance(GPS_CoordLead, GPS_NextWPCoord, &GPS_DistanceToWP);
                GPS_CalcLocationError(GPS_NextWPCoord, GPS_CoordLead);
            }
            else
            {
                GPS_CalcDistance(GPS_CurrentCoord, GPS_NextWPCoord, &GPS_DistanceToWP);
                GPS_CalcLocationError(GPS_NextWPCoord, GPS_CurrentCoord);
            }

            /* Adjust altitude */
            /* if we are holding position and reached target altitude, then ignore altitude nav, and let the user trim alt */
            if ( !((NAV_state == NAV_STATE_HOLD_INFINIT) && (Alt_ChangeFlag == ALT_FLAG_REACHED_ALT)))
            {
                if (!globalFlags.LAND_IN_PROGRESS)
                {
                    GPS_AltitudeHold = GPS_GetNewAltitude();
                    AltHold = GPS_AltitudeHold;
                }
            }

            /* Desired navigation speed */
            int16_t speed = 0;

            /* Navigation state machine */
            switch (NAV_state)
            {
            case NAV_STATE_NONE:
                /* Just for clarity, do nothing when nav_state is none */
                break;

            case NAV_STATE_LAND_START:
                /* Land in position hold */
                GPS_CalcPositionHold();
                GPS_LandSettleTimer = millis();
                NAV_state = NAV_STATE_LAND_SETTLE;
                break;

            case NAV_STATE_LAND_SETTLE:
                GPS_CalcPositionHold();
                if (millis() - GPS_LandSettleTimer > GPS_SETTLE_TIME_BEFORE_LAND_MS)
                {
                    NAV_state = NAV_STATE_LAND_START_DESCENT;
                }
                break;

            case NAV_STATE_LAND_START_DESCENT:
                GPS_CalcPositionHold();                              /* Land in position hold */
                globalFlags.THROTTLE_IGNORED = 1;                    /* Ignore Throtte stick input */
                globalFlags.GPS_BARO_MODE    = 1;                    /* Take control of BARO mode */
                NAV_LandDetectCounter = 0;                           /* Reset land detector */
                globalFlags.LAND_COMPLETED = 0;
                globalFlags.LAND_IN_PROGRESS = 1;                    /* Flag land process */
                NAV_state = NAV_STATE_LAND_IN_PROGRESS;
                break;

            case NAV_STATE_LAND_IN_PROGRESS:
                GPS_CalcPositionHold();
                GPS_CheckIfLanded();                                 /* Call land detector */
                if (globalFlags.LAND_COMPLETED)
                {
                    NAV_TimerStop = millis() + GPS_DISARM_TIME_AFTER_LAND_MS;
                    NAV_state = NAV_STATE_LANDED;
                }
                break;

            case NAV_STATE_LANDED:
                /* Disarm if THROTTLE stick is at minimum or 5sec past after land detected */
                if ((rcData[THROTTLE] < RC_MINCHECK) || (NAV_TimerStop <= millis()))
                {
                    /* Throttle at minimum or 5sec passed. */
                    FlightController_SystemDisarm();
                    globalFlags.OK_TO_ARM = 0;                       /* Prevent rearming */
                    NAV_state = NAV_STATE_NONE;                      /* Disable position holding.... prevent flippover */
                    globalFlags.GPS_BARO_MODE = 0;
                    globalFlags.LAND_COMPLETED = 0;
                    globalFlags.LAND_IN_PROGRESS = 0;
                    NAV_LandDetectCounter = 0;
                    globalFlags.THROTTLE_IGNORED = 0;
                    GPS_ResetNavigation();
                }
                break;

            case NAV_STATE_HOLD_INFINIT:
                /* Constant position hold, no timer. Only an rcOption change can exit from this */
                GPS_CalcPositionHold();
                break;

            case NAV_STATE_HOLD_TIMED:
                if (NAV_TimerStop == 0)                             /* We are start a timed poshold */
                {
                    NAV_TimerStop = millis() + 1000 * NAV_HoldTime; /* Set when we will continue */
                }
                else if (NAV_TimerStop <= millis())                 /* did we reach our time limit ? */
                {
                    if (WaypointRecord.flag != MISSION_FLAG_END)
                    {
                        NAV_state = NAV_STATE_PROCESS_NEXT;         /* if yes then process next mission step */
                    }

                    NAV_error = NAV_ERROR_TIMEWAIT;
                }
                GPS_CalcPositionHold();                             /* BTW hold position till next command */
                break;

            case NAV_STATE_RTH_START:
                /* Wait until we reach RTH altitude */
                if ((Alt_ChangeFlag == ALT_FLAG_REACHED_ALT) || (!GPS_Config.wait_for_rth_alt))
                {
                    /* If we reached then change mode and start RTH */
                    GPS_SetNextWP(GPS_HomeCoord, GPS_CurrentCoord);
                    NAV_state = NAV_STATE_RTH_ENROUTE;
                    NAV_error = NAV_ERROR_NONE;
                }
                else
                {
                    /* hold position till we reach RTH alt */
                    GPS_CalcPositionHold();
                    NAV_error = NAV_ERROR_WAIT_FOR_RTH_ALT;
                }
                break;

            case NAV_STATE_RTH_ENROUTE: /* Doing RTH navigation */
                speed = GPS_CalcDesiredSpeed(GPS_Config.nav_speed_max, GPS_Config.slow_nav);
                GPS_CalcNavRate(speed);
                GPS_AdjustHeading();

                if ((GPS_DistanceToWP <= GPS_Config.wp_radius) || GPS_CheckMissedWP())
                {
                    /* If yes switch to poshold mode */
                    if (WaypointRecord.parameter1 == 0)
                    {
                        NAV_state = NAV_STATE_HOLD_INFINIT;
                    }
                    else
                    {
                        /* if parameter 1 in RTH step is non 0 then land at home */
                        NAV_state = NAV_STATE_LAND_START;
                    }
                    if (GPS_Config.nav_rth_takeoff_heading)
                    {
                        magHold = GPS_TakeoffBearing;
                    }
                }
                break;

            case NAV_STATE_WP_ENROUTE:
                speed = GPS_CalcDesiredSpeed(GPS_Config.nav_speed_max, GPS_Config.slow_nav);
                GPS_CalcNavRate(speed);
                GPS_AdjustHeading();

                /* This decides what happen when we reached the WP coordinates */
                if ((GPS_DistanceToWP <= GPS_Config.wp_radius) || GPS_CheckMissedWP())
                {
                    if (WaypointRecord.action == MISSION_LAND)      /* Autoland */
                    {
                        /* Start landing */
                        NAV_state = NAV_STATE_LAND_START;

                        /* Stop any altitude changes */
                        GPS_SetNewAltitude(alt.estAltitude);
                    }
                    else if (WaypointRecord.flag == MISSION_FLAG_END)
                    {
                        /* If this was the last mission step (flag set by the mission planner), then switch to poshold */
                        NAV_state = NAV_STATE_HOLD_INFINIT;
                        NAV_error = NAV_ERROR_FINISH;
                    }
                    else if (WaypointRecord.action == MISSION_HOLD_UNLIM)
                    {
                        /* If WaypointRecord was POSHOLD_UNLIM and we reached the position then switch to poshold unlimited */
                        NAV_state = NAV_STATE_HOLD_INFINIT;
                        NAV_error = NAV_ERROR_FINISH;

                    }
                    else if (WaypointRecord.action == MISSION_HOLD_TIME)
                    {
                        /* If WaypointRecord was a timed poshold then initiate timed poshold */
                        NAV_HoldTime = WaypointRecord.parameter1;

                        /* This indicates that we are starting a timed poshold */
                        NAV_TimerStop = 0;
                        NAV_state = NAV_STATE_HOLD_TIMED;
                    }
                    else
                    {
                        /* Otherwise process next step */
                        NAV_state = NAV_STATE_PROCESS_NEXT;
                    }
                }
                break;

            case NAV_STATE_DO_JUMP:
                /* Jump unconditionally (supposed to be -1) -10 should not be here */
                if (NAV_JumpTimes < 0)
                {
                    NAV_NextStep = WaypointRecord.parameter1;
                    NAV_state = NAV_STATE_PROCESS_NEXT;
                }

                if (NAV_JumpTimes == 0)
                {
                    /* reset jump counter */
                    NAV_JumpTimes = -10;

                    /* If this was the last mission step (flag set by the mission planner), then switch to poshold */
                    if (WaypointRecord.flag == MISSION_FLAG_END)
                    {
                        NAV_state = NAV_STATE_HOLD_INFINIT;
                        NAV_error = NAV_ERROR_FINISH;
                    }
                    else
                    {
                        NAV_state = NAV_STATE_PROCESS_NEXT;
                    }
                }

                /* If zero not reached do a jump */
                if (NAV_JumpTimes > 0)
                {
                    NAV_NextStep = WaypointRecord.parameter1;
                    NAV_state = NAV_STATE_PROCESS_NEXT;
                    NAV_JumpTimes--;
                }
                break;


            case NAV_STATE_PROCESS_NEXT:
                /* Processing next mission step */
                NAV_error = NAV_ERROR_NONE;

                if (!EEPROM_ReadWPRecord(NAV_NextStep))
                {
                    GPS_AbortMission(NAV_ERROR_WP_CRC);
                }
                else
                {
                    switch(WaypointRecord.action)
                    {
                        /* Waypoint and hold commands all starts with an enroute status it includes the LAND command too */
                        case MISSION_WAYPOINT:
                        case MISSION_HOLD_TIME:
                        case MISSION_HOLD_UNLIM:
                        case MISSION_LAND:
                            GPS_SetNewAltitude(WaypointRecord.altitude);
                            GPS_SetNextWP(WaypointRecord.pos, GPS_PrevWPCoord);

                            if ((GPS_DistanceToWP/100) >= GPS_Config.safe_wp_distance)
                            {
                                GPS_AbortMission(NAV_ERROR_TOOFAR);
                            }
                            else
                            {
                                NAV_state = NAV_STATE_WP_ENROUTE;
                            }

                            /* Save WP coordinates for precise route calc */
                            GPS_PrevWPCoord[LAT] = WaypointRecord.pos[LAT];
                            GPS_PrevWPCoord[LON] = WaypointRecord.pos[LON];
                            break;

                        case MISSION_RTH:
                            globalFlags.GPS_HEAD_SET = 0;

                            if ((GPS_Config.rth_altitude == 0) && (WaypointRecord.altitude == 0))
                            {
                                /* if config and WaypointRecord alt is zero RTH returns at the actual altitude */
                                GPS_SetNewAltitude(alt.estAltitude);
                            }
                            else
                            {
                                uint32_t rth_alt;
                                if (WaypointRecord.altitude == 0)
                                {
                                    /* altitude in mission step has priority */
                                    rth_alt = GPS_Config.rth_altitude * 100;
                                }
                                else
                                {
                                    rth_alt = WaypointRecord.altitude;
                                }

                                /* BUt only if we are below it. */
                                if (alt.estAltitude < rth_alt)
                                {
                                    GPS_SetNewAltitude(rth_alt);
                                }
                                else
                                {
                                    GPS_SetNewAltitude(alt.estAltitude);
                                }
                            }

                            NAV_state = NAV_STATE_RTH_START;
                            break;

                        case MISSION_JUMP:
                            if (NAV_JumpTimes == -10)
                            {
                                NAV_JumpTimes = WaypointRecord.parameter2;
                            }

                            if ((WaypointRecord.parameter1 > 0) && (WaypointRecord.parameter1 < WaypointRecord.number))
                            {
                                NAV_state = NAV_STATE_DO_JUMP;
                            }
                            else
                            {
                                /* Error situation, invalid jump target */
                                GPS_AbortMission(NAV_ERROR_INVALID_JUMP);
                            }
                            break;

                        case MISSION_SET_HEADING:
                            /* zeroing this out clears the possible pervious set_poi */
                            GPS_POICoord[LAT] = 0; GPS_POICoord[LON] = 0;

                            if (WaypointRecord.parameter1 < 0)
                            {
                                globalFlags.GPS_HEAD_SET = 0;
                            }
                            else
                            {
                                globalFlags.GPS_HEAD_SET = 1;
                                GPS_DirectionToPOI = WaypointRecord.parameter1;
                            }
                            break;

                        case MISSION_SET_POI:
                            GPS_POICoord[LAT] = WaypointRecord.pos[LAT];
                            GPS_POICoord[LON] = WaypointRecord.pos[LON];
                            globalFlags.GPS_HEAD_SET = 1;
                            break;

                        /* if we got an unknown action code abort mission and hold position */
                        default:
                            GPS_AbortMission(NAV_ERROR_INVALID_DATA);
                            break;
                    }
                    /* Prepare for the next step */
                    NAV_NextStep++;
                }
                break;
            }
        } /* End of Navigation state machine */
    } /* End of FIX check */
}

/*******************************************************************************
* Function Name: GPS_ResetHomePosition
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void GPS_ResetHomePosition(void)
{
    if ((globalFlags.GPS_FIX) && (GPS_NumSat >= GPS_STABLE_MIN_SATELLITE_NUM))
    {
#if defined(I2C_GPS)
        /* set current position as home */
        GPS_I2C_command(I2C_GPS_COMMAND_SET_WP, 0);  /* WP0 is the home position */
#else /* defined(I2C_GPS) */

        GPS_HomeCoord[LAT] = GPS_CurrentCoord[LAT];
        GPS_HomeCoord[LON] = GPS_CurrentCoord[LON];
        GPS_CalcLongitudeScaling(GPS_CurrentCoord[LAT]);  /* need an initial value for distance and bearing calc */
#endif /* defined(I2C_GPS) */

        /* save takeoff heading */
        GPS_TakeoffBearing = att.heading;

        /* Set ground altitude */
        globalFlags.GPS_FIX_HOME = 1;
    }
}

/*******************************************************************************
* Function Name: GPS_InitRTH
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void GPS_InitRTH(void)
{
    globalFlags.GPS_MODE = GPS_MODE_RTH;            /* Set GPS_mode to RTH */
    globalFlags.GPS_BARO_MODE = true;

    GPS_HoldCoord[LAT] = GPS_CurrentCoord[LAT];     /* All RTH starts with a poshold */
    GPS_HoldCoord[LON] = GPS_CurrentCoord[LON];     /* This allows to raise to RTH altitude */
    GPS_SetNextWP(GPS_HoldCoord, GPS_HoldCoord);

    NAV_PauseAtStep = 0;

    if (GPS_Config.rth_altitude == 0)
    {
        GPS_SetNewAltitude(alt.estAltitude);        /* Return at actual altitude */
    }
    else
    {
        /*  RTH altitude is defined, but we use it only if we are below it */
        if (alt.estAltitude < GPS_Config.rth_altitude * 100)
        {
            GPS_SetNewAltitude(GPS_Config.rth_altitude * 100);
        }
        else
        {
            GPS_SetNewAltitude(alt.estAltitude);
        }
    }

    globalFlags.GPS_HEAD_SET = 0;                   /* Allow the RTH ti handle heading */
    NAV_state = NAV_STATE_RTH_START;                /* NAV engine status is Starting RTH. */
}

/*******************************************************************************
* Function Name: GPS_AdjustHeading
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void GPS_AdjustHeading(void)
{
    /* This controls the heading */
    if (globalFlags.GPS_HEAD_SET)                 /* We have seen a SET_POI or a SET_HEADING command */
    {

        if (GPS_POICoord[LAT] == 0)
        {
            magHold = GPS_DirectionToPOI-180;
        }
        else
        {
            GPS_CalcBearing(GPS_CurrentCoord, GPS_POICoord, &GPS_DirectionToPOI);
            GPS_CalcDistance(GPS_CurrentCoord, GPS_POICoord, &GPS_DistanceToWP);
            magHold = GPS_DirectionToPOI /100;
        }
    }
    else if (GPS_Config.nav_controls_heading)
    {
        /* heading controlled by the standard defines */
        if (GPS_Config.nav_tail_first)
        {
            magHold = wrap_18000(GPS_TargetBearing - 18000) / 100;
        }
        else
        {
            magHold = GPS_TargetBearing / 100;
        }
    }
}

/*******************************************************************************
* Function Name: GPS_CalculateAngles
********************************************************************************
* Summary:
*  Calculate the angles that should be added to the RC data
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void GPS_CalculateAngles(void)
{
    if ((globalFlags.GPS_MODE != GPS_MODE_NONE) && globalFlags.GPS_FIX_HOME)
    {
        float sin_yaw_y = sin(att.heading * RADX);
        float cos_yaw_x = cos(att.heading * RADX);

        GPS_Angle[ROLL]   = (NAV_Bank[LON] * cos_yaw_x - NAV_Bank[LAT] * sin_yaw_y) / 10;
        GPS_Angle[PITCH]  = (NAV_Bank[LON] * sin_yaw_y + NAV_Bank[LAT] * cos_yaw_x) / 10;
    }
    else
    {
        GPS_Angle[ROLL]  = 0;
        GPS_Angle[PITCH] = 0;
    }
}

/*******************************************************************************
* Function Name: GPS_CalcLongitudeScaling
********************************************************************************
* Summary:
*  This is used to offset the shrinking longitude as we go towards the poles.
*  It's ok to calculate this once per waypoint setting, since it changes a little
*  within the reach of a multicopter.
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void GPS_CalcLongitudeScaling(int32_t lattitude)
{
    GPS_scaleLonDown = cos(lattitude * 1.0e-7f * 0.01745329251f);
}

/*******************************************************************************
* Function Name: GPS_CalcDistance
********************************************************************************
* Summary:
*  Get distance between two points in cm,
*
* Parameters:
*  Copter curremt LAT and LON coordinates
*  Target LAT and LON coordinates
*
* Return:
*  Distance to the target point.
*
*******************************************************************************/
static void GPS_CalcDistance(int32_t* fromCoord, int32_t * toCoord, uint32_t* dist)
{
    float dLat = toCoord[LAT] - fromCoord[LAT];
    float dLon = (float)(toCoord[LON] - fromCoord[LON]) * GPS_scaleLonDown;

    *dist = sqrt(sq(dLat) + sq(dLon)) * 1.113195;
}

/*******************************************************************************
* Function Name: GPS_CalcBearing
********************************************************************************
* Summary:
*  Get bearing from pos1 to pos2, returns an 1deg = 100 precision.
*
* Parameters:
*  Copter curremt LAT and LON coordinates
*  Target LAT and LON coordinates
*
* Return:
*  Direction to the target point.
*
*******************************************************************************/
static void GPS_CalcBearing(int32_t* fromCoord, int32_t * toCoord, int32_t* bearing)
{
    float dLon = (toCoord[LON] - fromCoord[LON]);
    float dLat = (toCoord[LAT] - fromCoord[LAT]) / GPS_scaleLonDown;

    *bearing = 9000.0f + atan2(-dLat, dLon) * 5729.57795f;      /* Convert the output radians to 100xdeg */

    if (*bearing < 0)
    {
        *bearing += 36000;
    }
}

/*******************************************************************************
* Function Name: GPS_CalcVelocity
********************************************************************************
* Summary:
*  calc_velocity_and_filtered_position - velocity in lon and lat directions
*  calculated from GPS position and accelerometer data
*  lon_speed expressed in cm/s.  positive numbers mean moving east
*  lat_speed expressed in cm/s.  positive numbers when moving north
*
* Note: we use gps locations directly to calculate velocity instead of asking
* 		gps for velocity because this is more accurate below 1.5m/s
* Note: even though the positions are projected using a lead filter, the velocities
* 		are calculated from the unaltered gps locations.  We do not want noise
* 		from our lead filter affecting velocity
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void GPS_CalcVelocity(void)
{
    static int16_t prevSpeed[2] = {0, 0};
    static int32_t prevCoord[2] = {0, 0};
    static uint8_t initDone = 0;

    if (initDone)
    {
        float tmp = 1.0 / GPS_dTnav;
        GPS_ActualSpeed[X_AXIS] = (float)(GPS_CurrentCoord[LON] - prevCoord[LON]) *  GPS_scaleLonDown * tmp;
        GPS_ActualSpeed[Y_AXIS] = (float)(GPS_CurrentCoord[LAT] - prevCoord[LAT]) * tmp;

        if (!GPS_Config.lead_filter)
        {
            GPS_ActualSpeed[X_AXIS] = (GPS_ActualSpeed[X_AXIS] + prevSpeed[X_AXIS]) >> 1;
            GPS_ActualSpeed[Y_AXIS] = (GPS_ActualSpeed[Y_AXIS] + prevSpeed[Y_AXIS]) >> 1;
        }

        prevSpeed[X_AXIS] = GPS_ActualSpeed[X_AXIS];
        prevSpeed[Y_AXIS] = GPS_ActualSpeed[Y_AXIS];
    }

    initDone = 1;

    /* Save previous coordinates */
    prevCoord[LON] = GPS_CurrentCoord[LON];
    prevCoord[LAT] = GPS_CurrentCoord[LAT];

    /* Predict nex position based on the velocity and acceleration */
    if (GPS_Config.lead_filter)
    {
        GPS_CoordLead[LON] = GPS_LeadFilter_GetPosition(GPS_CurrentCoord[LON], GPS_ActualSpeed[X_AXIS], prevSpeed[X_AXIS], GPS_LAG);
        GPS_CoordLead[LAT] = GPS_LeadFilter_GetPosition(GPS_CurrentCoord[LAT], GPS_ActualSpeed[Y_AXIS], prevSpeed[Y_AXIS], GPS_LAG);
    }
}

/*******************************************************************************
* Function Name: GPS_CalcVelocity
********************************************************************************
* Summary:
*  This function predicts the next GPS position taking to account velocity,
*  acceleration and lag in seconds (time).
*  pos = pos_actual + v*t + dV*t^2
*
* Parameters:
*  int32_t pos - actual position
*  int16_t vel - current actual speed
*  int16_t prevVel - speed value from previous cycle
*  float lag_in_seconds - constant that depends on the specific hardware
*
* Return:
*  None
*
*******************************************************************************/
static int32_t GPS_LeadFilter_GetPosition(int32_t pos, int16_t vel, int16_t prevVel, float lag_in_seconds)
{
    int16_t accel_contribution = (vel - prevVel) * lag_in_seconds * lag_in_seconds;
    int16_t vel_contribution = vel * lag_in_seconds;

    return (pos + vel_contribution + accel_contribution);
}

/*******************************************************************************
* Function Name: GPS_CalcLocationError
********************************************************************************
* Summary:
*  Calculate a location error between two gps coordinates
*  Because we are using lat and lon to do our distance errors here's a quick chart:
*    100  = 1m     =   3 feet
*   1000  = 11m    =  36 feet
*   1800  = 19.80m =  60 feet
*   3000  = 33m    = 100 feet
*  10000  = 111m   = 333 feet
*
* Parameters:
*
*
* Return:
*  None
*
*******************************************************************************/
static void GPS_CalcLocationError( int32_t* targetCoord, int32_t* currentCoord )
{
    GPS_PosError[LON] = (float)(targetCoord[LON] - currentCoord[LON]) * GPS_scaleLonDown;  /* X Error */
    GPS_PosError[LAT] = targetCoord[LAT] - currentCoord[LAT]; /* Y Error */
}

/*******************************************************************************
* Function Name: GPS_CalcLocationError
********************************************************************************
* Summary:
*  Calculate nav_lat and nav_lon from the x and y error and the speed
*
* Parameters:
*
*
* Return:
*  None
*
*******************************************************************************/
static void GPS_CalcPositionHold(void)
{
    int32_t d;
    int32_t target_speed;
    uint8_t axis;

    for (axis = 0; axis < 2; axis++)
    {
        /* calculate desired speed from lat/lon error */
        target_speed = GPS_GetPID_P(GPS_PosError[axis], &poshold_PID_PARAM);
        /* Constrain the target speed in poshold mode to NAV_POSHOLD_MAX_SPEED = 1m/s it helps avoid runaways.. */
        target_speed = constrain(target_speed,-NAV_POSHOLD_MAX_SPEED, NAV_POSHOLD_MAX_SPEED);

        GPS_RateError[axis] = target_speed - GPS_ActualSpeed[axis]; /* calc the speed error */

        NAV_Bank[axis] = GPS_GetPID_P(GPS_RateError[axis], &posholdRate_PID_PARAM)
                  + GPS_GetPID_I(GPS_RateError[axis] + GPS_PosError[axis], &GPS_dTnav, &posholdRatePID[axis], &posholdRate_PID_PARAM);

        d = GPS_GetPID_D(GPS_PosError[axis], &GPS_dTnav, &posholdRatePID[axis], &posholdRate_PID_PARAM);
        d = constrain(d, -2000, 2000);

        /* Get rid of the noise */
        if(abs(GPS_ActualSpeed[axis]) < 50)
        {
            d = 0;
        }

        NAV_Bank[axis] +=d;
        NAV_Bank[axis]  = constrain_int16(NAV_Bank[axis], -GPS_Config.nav_bank_max, GPS_Config.nav_bank_max);
        navPID[axis].integrator = posholdRatePID[axis].integrator;
    }
}


/*******************************************************************************
* Function Name: GPS_CalcLocationError
********************************************************************************
* Summary:
*   Determine desired speed when navigating towards a waypoint, also implement slow
*   speed rampup when starting a navigation
*
*       |< WP Radius
*       0  1   2   3   4   5   6   7   8m
*       ...|...|...|...|...|...|...|...|
*                 100  |  200     300     400cm/s
*                  |                                        +|+
*                  |< we should slow to 1.5 m/s as we hit the target
*
* Parameters:
*	uint16_t max_speed - maximum speed value
*
* Return:
*  None
*
*******************************************************************************/
static uint16_t GPS_CalcDesiredSpeed(uint16_t max_speed, bool slow)
{
    /* max_speed is default 400 or 4m/s */
    if (slow)
    {
        max_speed = min(max_speed, GPS_DistanceToWP / 2);
    }
    else
    {
        /* Limit the speed based on the distance to WP to prevent overshoot */
        max_speed = min(max_speed, GPS_DistanceToWP);

        /* go at least 100cm/s */
        max_speed = max(max_speed, GPS_Config.nav_speed_min);
    }

    /* limit the ramp up of the speed */
    /* GPSNav.WP_SpeedGov is reset to 0 at each new WP command */
    if (max_speed > NAV_WP_SpeedGov)
    {
        /* increase at .5m /s */
        NAV_WP_SpeedGov += (int)(100.0 * GPS_dTnav);
        max_speed = NAV_WP_SpeedGov;
    }

    return max_speed;
}

/*******************************************************************************
* Function Name: GPS_CalcNavRate
********************************************************************************
* Summary:
*   Calculate the desired nav_lat and nav_lon for distance flying such as RTH
*
* Parameters:
*	uint16_t max_speed - maximum speed value
*
* Return:
*  None
*
*******************************************************************************/
static void GPS_CalcNavRate(uint16_t max_speed)
{
    float trig[2];
    int32_t target_speed[2];
    int32_t tilt;
    uint8_t axis;

    /* push us towards the original track */
    GPS_UpdateCrosstrack();

    int16_t cross_speed = GPS_CosstrackError * (GPS_Config.crosstrack_gain / 100.0);  /* check is it ok ? */
    cross_speed = constrain(cross_speed,-200,200);
    cross_speed = -cross_speed;

    float temp = (9000l - GPS_TargetBearing) * RADX100;
    trig[X_AXIS] = cos(temp);
    trig[Y_AXIS] = sin(temp);

    target_speed[X_AXIS] = max_speed * trig[X_AXIS] - cross_speed * trig[Y_AXIS];
    target_speed[Y_AXIS] = cross_speed * trig[X_AXIS] + max_speed * trig[Y_AXIS];

    for (axis=0;axis<2;axis++)
    {
        GPS_RateError[axis] = target_speed[axis] - GPS_ActualSpeed[axis];
        GPS_RateError[axis] = constrain(GPS_RateError[axis],-1000,1000);
        /* P + I + D */
        NAV_Bank[axis] = GPS_GetPID_P(GPS_RateError[axis], &nav_PID_PARAM)
                         + GPS_GetPID_I(GPS_RateError[axis], &GPS_dTnav, &navPID[axis], &nav_PID_PARAM)
                         + GPS_GetPID_D(GPS_RateError[axis], &GPS_dTnav, &navPID[axis], &nav_PID_PARAM);

        NAV_Bank[axis]  = constrain_int16(NAV_Bank[axis], -GPS_Config.nav_bank_max, GPS_Config.nav_bank_max);
        posholdRatePID[axis].integrator = navPID[axis].integrator;
    }
}

/*******************************************************************************
* Function Name: GPS_UpdateCrosstrack
********************************************************************************
* Summary:
*   Calculating cross track error, this tries to keep the copter on a direct line
*   when flying to a waypoint.
*
* Parameters:
*	uint16_t max_speed - maximum speed value
*
* Return:
*  None
*
*******************************************************************************/
static void GPS_UpdateCrosstrack(void)
{
    /* If we are too far off or too close we don't do track following */
    float temp = (GPS_TargetBearing - GPS_OriginalTargetBearing) * RADX100;
    GPS_CosstrackError = sin(temp) * GPS_DistanceToWP;  /* Meters we are off track line */
}

/*******************************************************************************
* Function Name: wrap_36000
********************************************************************************
* Summary:
*
*
* Parameters:
*   int32_t ang - angle in 100x degrees
*
* Return:
*  None
*
*******************************************************************************/
static int32_t wrap_36000(int32_t ang)
{
    if (ang > 36000)
    {
        ang -= 36000;
    }

    if (ang < 0)
    {
        ang += 36000;
    }

    return ang;
}

/*******************************************************************************
* Function Name: wrap_18000
********************************************************************************
* Summary:
*
*
* Parameters:
*   int32_t ang - angle in 100x degrees
*
* Return:
*  None
*
*******************************************************************************/
static int32_t wrap_18000(int32_t ang)
{
    if (ang > 18000)
    {
        ang -= 36000;
    }

    if (ang < -18000)
    {
        ang += 36000;
    }

    return ang;
}

/*******************************************************************************
* Function Name: wrap_18000
********************************************************************************
* Summary:
*
*
* Parameters:
*   int32_t ang - angle in 100x degrees
*
* Return:
*  None
*
*******************************************************************************/
static bool GPS_CheckMissedWP(void)
{
    int32_t temp;
    temp = GPS_TargetBearing - GPS_OriginalTargetBearing;
    temp = wrap_18000(temp);
    return (abs(temp) > 10000);   /* we passed the waypoint by 100 degrees */
}

/*******************************************************************************
* Function Name: GPS_SetNextWP
********************************************************************************
* Summary:
*   Sets the waypoint to navigate, reset neccessary variables and calculate initial values
*
* Parameters:
*
* Return:
*  None
*
*******************************************************************************/
void GPS_SetNextWP(int32_t* newWPCoord, int32_t* currentWPCoord)
{
    GPS_NextWPCoord[LAT] = newWPCoord[LAT];
    GPS_NextWPCoord[LON] = newWPCoord[LAT];

    GPS_CalcLongitudeScaling(GPS_NextWPCoord[LAT]);
    GPS_CalcBearing(currentWPCoord, GPS_NextWPCoord, &GPS_TargetBearing);
    GPS_CalcDistance(currentWPCoord, GPS_NextWPCoord, &GPS_DistanceToWP);
    GPS_CalcLocationError(GPS_NextWPCoord, currentWPCoord);

    GPS_OriginalTargetBearing = GPS_TargetBearing;
    NAV_WP_SpeedGov = GPS_Config.nav_speed_min;
}

/*******************************************************************************
* Function Name: GPS_ResetNavigation
********************************************************************************
* Summary:
*   reset navigation (stop the navigation processor, and clear nav)
*
* Parameters:
*
* Return:
*  None
*
*******************************************************************************/
void GPS_ResetNavigation(void)
{
    uint8_t i;

    for (i = 0; i < 2; i++)
    {
        NAV_Bank[i] = 0;
        GPS_ResetPID(&posholdPID[i]);
        GPS_ResetPID(&posholdRatePID[i]);
        GPS_ResetPID(&navPID[i]);

        NAV_JumpTimes = -10;
        /* reset next step counter */
        NAV_NextStep = 1;

        /* Clear poi */
        GPS_POICoord[LAT] = 0;
        GPS_POICoord[LON] = 0;
        globalFlags.GPS_HEAD_SET = 0;
    }
}

/********************************************************************************
* Function Name: GPS_GetNewAltitude
*********************************************************************************
* Summary:
*  returns a new altitude which feeded into the alt.hold controller
*
* Parameters:
*  None
*
* Return:
*  None
*
*********************************************************************************/
int32_t GPS_GetNewAltitude(void)
{
    if (Alt_ChangeFlag == ALT_FLAG_ASCENDING)
    {
        /* we are below, going up */
        if (alt.estAltitude >=  NAV_TargetAltitude)
        {
            Alt_ChangeFlag = ALT_FLAG_REACHED_ALT;
        }

        /* we shouldn't command past our target */
        if (GPS_AltitudeHold >=  NAV_TargetAltitude)
        {
            return NAV_TargetAltitude;
        }
    }
    else if (Alt_ChangeFlag == ALT_FLAG_DESCENDING)
    {
        /* we are above, going down */
        if (alt.estAltitude <=  NAV_TargetAltitude)
        {
            Alt_ChangeFlag = ALT_FLAG_REACHED_ALT;
        }

        /* we shouldn't command past our target */
        if (GPS_AltitudeHold <=  NAV_TargetAltitude)
        {
            return NAV_TargetAltitude;
        }
    }

    /* if we have reached our target altitude, return the target alt */
    if (Alt_ChangeFlag == ALT_FLAG_REACHED_ALT)
    {
        return NAV_TargetAltitude;
    }


    int32_t diff = abs(GPS_AltitudeHold - NAV_TargetAltitude);
    /* scale is how we generate a desired rate from the elapsed time */
    /* a smaller scale means faster rates */
    int8_t _scale = 4;

    if (GPS_AltitudeHold < NAV_TargetAltitude)
    {
        /* we are below the target alt */
        if (diff < 200)
        {
            _scale = 4;
        }
        else
        {
            _scale = 3;
        }
    }
    else
    {
        /* we are above the target, going down */
        if (diff < 400)
        {
            _scale = 5;      /* Slow down if only 4meters above */
        }

        if (diff < 100)
        {
            _scale = 6;      /* Slow down further if within 1meter */
        }
    }

    /* we use the elapsed time as our altitude offset */
    /* 1000 = 1 sec */
    /* 1000 >> 4 = 64cm/s descent by default */
    int32_t change = (millis() - Alt_ChangeTimer) >> _scale;

    if (Alt_ChangeFlag == ALT_FLAG_ASCENDING)
    {
        Alt_ChangeIntegrator += change;
    }
    else
    {
        Alt_ChangeIntegrator -= change;
    }

    /* For generating delta time */
    Alt_ChangeTimer = millis();

    return (NAV_OriginalAltitude + Alt_ChangeIntegrator);
}

/********************************************************************************
* Function Name: GPS_SetNewAltitude
*********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*********************************************************************************/
void GPS_SetNewAltitude(int32_t _new_alt)
{
    /* Limit maximum altitude command */
    if (_new_alt > GPS_Config.nav_max_altitude * 100)
    {
        _new_alt = GPS_Config.nav_max_altitude * 100;
    }

    if (_new_alt == alt.estAltitude)
    {
        GPS_ForceNewAltitude(_new_alt);
        return;
    }

    /* We start at the current location altitude and gradually change it */
    GPS_AltitudeHold = alt.estAltitude;

    /* For calculating the delta time */
    Alt_ChangeTimer = millis();

    /* Save the target altitude */
    NAV_TargetAltitude = _new_alt;

    /* Reset our altitude integrator */
    Alt_ChangeIntegrator = 0;

    /* Save the original altitude */
    NAV_OriginalAltitude = alt.estAltitude;

    /* To decide if we have reached the target altitude */
    if(NAV_TargetAltitude > NAV_OriginalAltitude)
    {
        /* we are below, going up */
        Alt_ChangeFlag = ALT_FLAG_ASCENDING;
    }
    else if(NAV_TargetAltitude < NAV_OriginalAltitude)
    {
        /* we are above, going down */
        Alt_ChangeFlag = ALT_FLAG_DESCENDING;
    }
    else
    {
        /* No Change */
        Alt_ChangeFlag = ALT_FLAG_REACHED_ALT;
    }
}

/********************************************************************************
* Function Name: GPS_GetAltitudeError
*********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*********************************************************************************/
static int32_t GPS_GetAltitudeError(void)
{
    return (GPS_AltitudeHold - alt.estAltitude);
}

/********************************************************************************
* Function Name: GPS_ClearNewAltitude
*********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*********************************************************************************/
static void GPS_ClearNewAltitude(void)
{
    Alt_ChangeFlag = ALT_FLAG_REACHED_ALT;
}

/********************************************************************************
* Function Name: GPS_ForceNewAltitude
*********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*********************************************************************************/
static void GPS_ForceNewAltitude(int32_t _new_alt)
{
    GPS_AltitudeHold   = _new_alt;
    NAV_TargetAltitude = _new_alt;
    Alt_ChangeFlag     = ALT_FLAG_REACHED_ALT;
}

/********************************************************************************
* Function Name: GPS_CheckIfLanded
*********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*********************************************************************************/
static void GPS_CheckIfLanded(void)
{
    /* detect whether we have landed by watching for low climb rate and throttle control */
    if ((abs(alt.estSpeed) < NAV_LAND_MIN_SPEED) && (BaroPID < NAV_LAND_BAROPIDMIN))
    {
        if (!globalFlags.LAND_COMPLETED)
        {

            if (NAV_LandDetectCounter < NAV_LAND_DETECT_THRESHOLD)
            {
                NAV_LandDetectCounter++;
            }
            else
            {
                globalFlags.LAND_COMPLETED = 1;
                NAV_LandDetectCounter = 0;
            }
        }
    }
    else
    {
        /* we've detected movement up or down so reset NAV_LandDetectCounteror */
        NAV_LandDetectCounter = 0;
        if(globalFlags.LAND_COMPLETED)
        {
            globalFlags.LAND_COMPLETED = 0;
        }
    }
}

/********************************************************************************
* Function Name: GPS_AbortMission
*********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*********************************************************************************/
static void GPS_AbortMission(unsigned char error_code)
{
    GPS_SetNextWP(GPS_CurrentCoord, GPS_CurrentCoord);
    NAV_error = error_code;
    NAV_state = NAV_STATE_HOLD_INFINIT;
}

/********************************************************************************
* Function Name: constrain_int16
*********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*********************************************************************************/
static int16_t constrain_int16(int16_t amt, int16_t low, int16_t high)
{
    return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
}
